#include "Marker.h"

Vec3d Marker::getWorldCoords(){
	Vec4d v(mCoords[0], mCoords[1], mCoords[2], 1);	
	Vec4d result = mBody->getWorldCoords(&v);
	Vec3d r;
	r[0]=result[0];
	r[1]=result[1];
	r[2]=result[2];
	return r;
}

Vec3d Marker::getPartialDeriv(Dof *d){
	Vec4d v(mCoords[0], mCoords[1], mCoords[2], 1);	
	Vec4d result = mBody->getFirstDerivative(&v, d);
	Vec3d r;
	r[0]=result[0];
	r[1]=result[1];
	r[2]=result[2];
	return r;
}

void Marker::draw(bool _noOffsetting, Vec4d _col, bool _default){
	glPushName(mEllipsoid->getID());
	if(mConstrained==Marker::NO) mEllipsoid->setColor(Vec3d(0, 0, 1));
	if(mConstrained==Marker::HARD) mEllipsoid->setColor(Vec3d(1, 0, 0));
	if(mConstrained==Marker::SOFT) mEllipsoid->setColor(Vec3d(0, 1, 0));
	
	if(!_noOffsetting){
		glPushMatrix();
		glTranslatef(mCoords[0], mCoords[1], mCoords[2]);
		mEllipsoid->draw(_col, _default);
		glPopMatrix();
	}
	else{
		mEllipsoid->draw(_col, _default);
	}
	glPopName();
}
